
from numpy import array
import ode
import math

class RobotController:
	def __init__(self):
		pass

class RobotPart:
	def __init__(self, size):
		self.size = size


class Robot:
	lines = []
	
	def __init__(self, brain):
		self.brain = brain
	
	def step(self, reference):
		pass
		#self.brain.step(self, reference)
		#self.hip_1.SetMotorSpeed(self.brain.hip_1)
		#self.hip_2.SetMotorSpeed(self.brain.hip_2)
		#self.knee_1.SetMotorSpeed(self.brain.knee_1)
		#self.knee_2.SetMotorSpeed(self.brain.knee_2)
	
	def get_center_of_gravity(self):
		return (0,0,0)
	
	def add_to_world(self, world, space):
		def addBody(pos, type):			
			body = ode.Body(world)
			M = ode.Mass()
			
			if type == "lowerleg":
				M.setCylinderTotal(0.120, 2, 0.014, 0.2555)
				body.boxsize = (0.014, 0.200, 0.014)
				body.shape = "box"
			elif type == "upperleg":
				M.setCylinderTotal(0.280, 2, 0.014, 0.254)
				body.boxsize = (0.014, 0.200, 0.014)
				body.shape = "box"
			elif type == "hip":
				M.setBoxTotal(0.030, 0.030, 0.030, 0.030)
				body.boxsize = (0.030, 0.030, 0.030)
				body.shape = "box"
			elif type == "torso":
				M.setBoxTotal(0.400, 0.150, 0.250, 0.040)
				body.boxsize = (0.150, 0.200, 0.040)
				body.shape = "box"
			
			body.setMass(M)
			body.setPosition(pos)
			
			# Create a box geom for collision detection
			geom = ode.GeomBox(space, lengths=body.boxsize)
			geom.setBody(body)

			return body
		
		offset = 0.025
		
		self.lower_leg_l = addBody((0.0467, 0.2555/2.0+offset, 0), "lowerleg")
		self.lower_leg_r = addBody((-0.0467, 0.2555/2.0+offset, 0), "lowerleg")
		self.upper_leg_l = addBody((0.0467, 0.3825+offset, 0), "upperleg")
		self.upper_leg_r = addBody((-0.0467, 0.3825+offset, 0), "upperleg")
		self.hip_l = addBody((0.0467, 0.5395+offset, 0), "hip")
		self.hip_r = addBody((-0.0467, 0.5395+offset, 0), "hip")
		self.torso = addBody((0, 0.7145+offset, -0.051), "torso")
		
		def addJoint(body1, body2, pos, axis, stopangles):
			joint = ode.HingeJoint(world)
			joint.attach(body1, body2)
			joint.setAnchor(pos)
			joint.setAxis(axis)
			joint.setParam(ode.ParamLoStop, stopangles[0])
			joint.setParam(ode.ParamHiStop, stopangles[1])
			#joint.setParam(ode.ParamVel, 3000)
			#joint.setParam(ode.ParamFMax, 20000)

			return joint
		
		self.hip_top_l = addJoint(self.torso, self.hip_l, (0.0467, 0.5695+offset, 0), (0,0,1), (-0.3,0.3))
		self.hip_top_r = addJoint(self.torso, self.hip_r, (-0.0467, 0.5695+offset, 0), (0,0,1), (-0.3,0.3))
		self.hip_bot_l = addJoint(self.hip_l, self.upper_leg_l, (0.0467, 0.5095+offset, 0), (1,0,0), (-1.3,1.3))
		self.hip_bot_r = addJoint(self.hip_r, self.upper_leg_r, (-0.0467, 0.5095+offset, 0), (1,0,0), (-1.3,1.3))
		self.knee_l = addJoint(self.upper_leg_l, self.lower_leg_l, (0.0467, 0.2555+offset, 0), (1,0,0), (-2.6,0))
		self.knee_r = addJoint(self.upper_leg_r, self.lower_leg_r, (-0.0467, 0.2555+offset, 0), (1,0,0), (-2.6,0))
